#pragma once

#include <string>
#include <fishbot_behavior_tree/check_error.hpp>
#include <fishbot_behavior_tree/fishbot_behavior_tree.hpp>

#include <nav2_msgs/action/navigate_to_pose.hpp>

using NavigationToPose = nav2_msgs::action::NavigateToPose;

namespace fishbot
{
    namespace behaviors
    {
        class Nav2PoseActionClient final : public fishbot::behavior_tree::ActionClientBehaviorBase<NavigationToPose>
        {
        public:
            Nav2PoseActionClient(const std::string &name, const BT::NodeConfiguration &config,
                                 const std::shared_ptr<fishbot::behavior_tree::BehaviorContext> &shared_resources);
            static BT::PortsList providedPorts();

        private:
            tl::expected<NavigationToPose::Goal, std::string> createGoal() override;

            tl::expected<bool, std::string> processResult(const std::shared_ptr<NavigationToPose::Result> result);
        };
    }
}
